﻿"""
cyprocess.pyx

This file is part of a program that implements a Software-Defined Radio.

Copyright (C) 2014 by G3UKB Bob Cowdery

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

The authors can be reached by email at:

	bob@bobcowdery.plus.com

"""

# Python imports
import sys, os
import threading
import socket
import select
import struct
import queue
import types
from time import sleep
import numpy as np
import logging
import time
import math

# Cython imports
cimport numpy as np
np.import_array()

# Application imports
from common.defs import *

# C declarations
cdef extern from "server.h" nogil:
	int c_server_exchange(int n_smpls, int n_rx, int rate, int in_sz, char *ptr_in_bytes, int out_sz, char *ptr_out_bytes)

# Lock for cc data
lock = threading.RLock()
		
"""
ProcessControl class:
	start metis threads
	API for H/W and DSP

"""

"""
Processor class: the orchestrator.
"""
class Processor(threading.Thread):
	
	# Processing stages:
	# 1. Sits on a select waiting for a data block from Metis.
	# 2. Reads Metis stream.
	# 3. Separates header, cc and data elements.
	# 4. Dispatches control elements to top level via given callback.
	# 5. Performs a data exchange with the fast pipeline processing.
	# 6. Create output block with merged control data.
	# 7. Writes output blocks to Metis.

	def __init__(self, control_cfg, metis_sock, metis_address, METIS_PORT, cc_in, cc_callback, q):
		
		"""
		Constructor
		
		Arguments:
			control_cfg			-- current configuration
			metis_sock			-- open socket to Metis hardware
			metis_address		-- IP address of Metis hardware
			METIS_PORT			-- Port Metis listens on
			cc_in				-- Array of cc arrays (10, 5)
			cc_callback			-- Callback here with cc data
			q					-- Q to send messages on
		"""
		
		threading.Thread.__init__(self)
		
		# Parameters  ==========================================================
		self.__control_cfg = control_cfg
		self.__metis_sock = metis_sock
		self.__metis_address = metis_address
		self.__METIS_PORT = METIS_PORT
		self.__cc_in = cc_in
		self.__cc_callback = cc_callback
		self.__q = q
		
		# Pipeline =============================================================
		
		# Class variables ======================================================
		# Terminate flag
		self.__terminate = False
		# Input sequence counters
		cdef unsigned int ep4_seq
		cdef unsigned int ep6_seq
		self.__ep4_seq = ep4_seq = 0
		self.__ep6_seq = ep6_seq = 0
		# Output sequence counter
		cdef unsigned int out_seq
		self.__out_seq = out_seq = 0
		# Current cc round-robin index
		self.__cc_index = 0
		self.__MAX_CC_INDEX = 10
		#self.__MAX_CC_INDEX = 3
		# Input data (Metis block sz)
		self.__udp_frame = np.zeros(METIS_BLK_SZ, dtype=np.uint8)
		# Raw data (2xframes for a Metis input block)
		self.__raw_data = np.zeros(SMPL_FRM_SZ*2, dtype=np.uint8)
		# Proc data frames (2xframes for a Metis output block)
		self.__proc_data = np.zeros(SMPL_FRM_SZ*2, dtype=np.uint8)
		# Output data (Metis block sz)
		self.__output_data = np.zeros(METIS_BLK_SZ, dtype=np.uint8)
		# HPSDR output header bytes
		self.__metis_header = np.array([0xef, 0xfe, 0x01, 0x02], dtype=np.ubyte)
		self.__usb_header = np.array([0x7f, 0x7f, 0x7f], dtype=np.ubyte)
		self.__done = False
		self.__counter = 0
		self.__starting = False
		
		np.set_printoptions(formatter={'int':hex})

	def terminate(self):
		
		""" Set termination """
		
		self.__terminate = True
		self.join()
	
	def run(self):		
		""" Run until termination """

		# Prime metis with two cycles of cc data
		for n in range(len(self.__cc_in)*2):
			self.__merge_fields()
			self.__writer()
		
		# This is the main loop of the processing.
		# Read a block, exchange a block, merge output fields, write a block
		self.__starting = True
		while not self.__terminate:
			# 1. Sits on a select waiting for a data block from Metis.
			# 2. Reads Metis stream.
			#while self.__reader() <> EP6:
			#	if self.__terminate:
			#		break
			#if self.__terminate:
			#		break
			
			endpoint = self.__reader()
			if endpoint == EP6:
				# 3. Separates header, cc and data elements.
				# Padding per frame for receivers 1-8
				padding = [0, 0, 4, 10, 24, 10, 20, 4]
				cc_out_0, cc_out_1, raw_data = self.__split_fields(padding[self.__control_cfg[COMMON][NUM_RX]-1])
				# 4. Dispatches cc data to top level via given callback.
				self.__cc_callback(cc_out_0)
				self.__cc_callback(cc_out_1)
				# 5. Performs a data exchange with the fast pipeline processing.
				# The server does some pre-processing of the data to get it in a better state for the pipeline
				# This processing can be considered to be part of this process, delegated for performance to C
				# The server needs to know the actual number of RX samples in the 2 frames
				#
				# Number of samples per frame for receivers 1-8
				smpls = [63, 36, 25, 19, 15, 13, 11, 10]
				# Do the data exchange
				r = __exchange(smpls[self.__control_cfg[COMMON][NUM_RX]-1]*2,
					self.__control_cfg[COMMON][NUM_RX],
					self.__control_cfg[COMMON][IN_RATE],
					len(raw_data - 2*padding[self.__control_cfg[COMMON][NUM_RX]-1]),
					np.PyArray_BYTES(raw_data),
					len(self.__proc_data),
					np.PyArray_BYTES(self.__proc_data))
				# We always exchange a full frames worth of input and output samples so the processing here is minimised
				# If we are priming or the sample rate is different then some exchanges will result in no output data.
				if r:
					# We have output data
					# 6. Create output block with merged control data.
					self.__merge_fields()

				# 7. Writes output block to Metis.
				self.__writer()
				self.__starting = False
			elif endpoint == EP4:
				# This data is passed to the C layer for digestion and then forms a callback to the Python layer with
				# formatted data ready for display according to the parameters passed into the WBS setup functions.
				# The C layer accumulates 16,384 words before processing. The frame start point is when the LS 5 bits
				# of the sequence number are zero.
				wbs_start = False
				if (self.__ep4_seq -1) & 0x1F == 0:
					wbs_start = True
				# Call C layer.
				# c_process_wbs(np.PyArray_BYTES(self.__udp_frame[SEQ_END:], wbs_start))			
		
	def __reader(self):
		
		""" Read data block from Metis """
		# Data from Metis:
		#
		# Metis samples block format:
		# <0xEFFE><0x01><End Point><Sequence Number><2 x HPSDR frames>
		# End point = 1 byte (0x06 - representing USB EP6)
		# Sequence Number = 4 bytes (32 bit unsigned integer starting at zero and incremented each
		# frame - bytes are in network order [big-endian])
		# HPSDR data = 1024 bytes (2 x 512 byte USB format frames)
		#
		# Metis wide-band scope block format
		# <0xEFFE><0x01><End Point><Sequence Number><512 x 16 bit samples>
		# End point = 1 byte (0x04 - representing USB EP4)
		# Sequence Number = 4 bytes (32 bit unsigned integer starting at zero and incremented each
		# frame and independently for each end point)
		# The PC should start reading data sent to EP4 when the least significant 3 bits of the
		# sequence number are zero. Then accumulate 4096 words and process them. This ensures
		# that the PC reads a complete buffer rather than reading across a buffer boundary.
		global toggle
		global t
		
		(rfd, wfd, efd) = select.select([self.__metis_sock], [], [], METIS_TIMEOUT)
		if self.__metis_sock in rfd:
			# Data available from Metis
			nbytes, address = self.__metis_sock.recvfrom_into(self.__udp_frame)
			if self.__udp_frame[ENDPOINT] == EP6: 
				# IQ data
				# Check and bump the sequence number
				seqb = self.__udp_frame[SEQ_ST:SEQ_END]
				seq =  (struct.unpack('>I', seqb.tostring()))[0]
				if seq != self.__ep6_seq and not self.__starting and not self.__terminate:
					self.__q.put ((BOTH, logging.INFO, 'cy.process', 'IQ sequence error, expected %d, got %d' % (self.__ep6_seq, seq)))
				# Set next seq
				self.__ep6_seq = seq+1
				return EP6
			elif self.__udp_frame[ENDPOINT] == EP4:
				# Wideband scope
				# Check and bump the sequence number
				seqb = self.__udp_frame[SEQ_ST:SEQ_END]
				seq =  (struct.unpack('>I', seqb.tostring()))[0]
				if seq != self.__ep4_seq and not self.__starting and not self.__terminate:
					self.__q.put ((BOTH, logging.INFO, 'cy.process', 'WBS sequence error, expected %d, got %d' % (self.__ep4_seq, seq)))
				# Set next seq
				self.__ep4_seq = seq+1
				return EP4
					
	def __split_fields(self, padding):		
		"""
		Separate the input fields into:
			Frame 1 control bytes
			Frame 2 control bytes
			Concatenated data from both frames
		
		Arguments:
			padding		--	number of padding bytes per frame for the number of receivers in use
		"""
		
		if not (self.__udp_frame[SYNC_0_ST:SYNC_0_END] == self.__usb_header).all() and not self.__starting and not self.__terminate:
			self.__q.put ((BOTH, logging.INFO, 'cy.process', 'Sync error'))
		if not (self.__udp_frame[SYNC_1_ST:SYNC_1_END] == self.__usb_header).all() and not self.__starting and not self.__terminate:
			self.__q.put ((BOTH, logging.INFO, 'cy.process', 'Sync error'))
		self.__raw_data[:SMPL_FRM_SZ - padding] = self.__udp_frame[FRM_0_ST:FRM_0_END - padding]
		self.__raw_data[SMPL_FRM_SZ - padding:len(self.__raw_data) - 2*padding] = self.__udp_frame[FRM_1_ST:FRM_1_END - padding]
		return 	self.__udp_frame[CTRL_0_ST:CTRL_0_END], \
				self.__udp_frame[CTRL_1_ST:CTRL_1_END], \
				self.__raw_data
	
	def __merge_fields(self):		
		""" Merge output fields into a Metis block """
		
		# Metis output format:
		# <0xEFFE><0x01><End Point><Sequence Number>< 2 x HPSDR frames>
		# Where:
		# End point = 1 byte (0x02 – representing USB EP2)
		# Sequence Number = 4 bytes (32 bit unsigned integer starting at zero and incremented each
		# 	frame – bytes are in network order [big-endian])
		# HPSDR data = 1024 bytes (2 x 512 byte USB format frames)
		#
		# The following fields are merged:
		# self.__metis_header
		# self.__out_seq		-- next output sequence number to use
		# self.__cc_in 			-- round robin control bytes
		# self.__cc_index		-- next index into cc
		# self.__MAX_CC_INDEX	-- max cc index
		# self.__usb_header +
		# self.__proc_data	-- 2 frames worth of USB format frames
		#
		# The fields are merged into self.__output_data
		#
		
		seq_bytes = list(struct.pack('>I', self.__out_seq))
		self.__out_seq += 1
		self.__output_data[:4] = self.__metis_header
		self.__output_data[4:8] = seq_bytes
		self.__output_data[8:11] = self.__usb_header
		self.__output_data[11:16] = self.__cc_in[self.__cc_index]
		self.__cc_index += 1
		if self.__cc_index > self.__MAX_CC_INDEX: self.__cc_index = 0
		self.__output_data[16:520] = self.__proc_data[:504]
		self.__output_data[520:523] = self.__usb_header
		self.__output_data[523:528] = self.__cc_in[self.__cc_index]
		self.__cc_index += 1
		if self.__cc_index > self.__MAX_CC_INDEX: self.__cc_index = 0
		self.__output_data[528:] = self.__proc_data[504:]
			
	def __writer(self):
		
		""" Write output data to Metis """
	
		(rfd, wfd, efd) = select.select([], [self.__metis_sock], [], METIS_TIMEOUT)
		if self.__metis_sock in wfd:
			self.__metis_sock.sendto(self.__output_data, (self.__metis_address, self.__METIS_PORT))
	
cdef __exchange(int n_smpls, int n_rx, int rate, int in_sz, char *ptr_in_bytes, int out_sz, char *ptr_out_bytes):
	"""
	Do data exchange without GIL. This improves concurrency.
	
	Arguments:
		n_smpls			--	number of I/Q samples per frame per receiver
		n_rx			--	number of receivers
		rate			-- 	48/96/192 K
		in_sz			--	size of input data buffer
		ptr_in_bytes   	--  ptr to the input data
		out_sz			--	size of output data buffer
		ptr_out_bytes  	--  pointer to a byte array to receive output data
	"""
	
	cdef int ret
	with nogil:
		ret = c_server_exchange(n_smpls, n_rx, rate, in_sz, ptr_in_bytes, out_sz, ptr_out_bytes)
		
	return ret
	